/*=========================================================================
 *
 *  Copyright NumFOCUS
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         https://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#include <iostream>

#include "itkMath.h"
#include "itkAffineTransform.h"
#include "itkStdStreamStateSave.h"

using Matrix2Type = itk::Matrix<double, 2, 2>;
using Vector2Type = itk::Vector<double, 2>;
using Matrix3Type = itk::Matrix<double, 3, 3>;

namespace
{

void
PrintVector(const Vector2Type & v)
{
  for (unsigned int i = 0; i < Vector2Type::Dimension; ++i)
  {
    std::cout << v[i] << ", ";
  }
  std::cout << std::endl;
}

bool
testValue(const double v1, const double v2, int maxUlps = 4)
{
  return itk::Math::FloatAlmostEqual(v1, v2, maxUlps);
}

template <typename TMatrix>
bool
testMatrix(const TMatrix & m1, const TMatrix & m2, int maxUlps = 4)
{
  bool pass = true;

  for (unsigned int i = 0; i < TMatrix::RowDimensions; ++i)
  {
    for (unsigned int j = 0; j < TMatrix::ColumnDimensions; ++j)
    {
      if (!testValue(m1[i][j], m2[i][j], maxUlps))
      {
        pass = false;
      }
    }
  }
  return pass;
}

template <typename TVector>
bool
testVector(const TVector & v1, const TVector & v2, int maxUlps = 4)
{
  bool pass = true;

  for (unsigned int i = 0; i < TVector::Dimension; ++i)
  {
    if (!testValue(v1[i], v2[i], maxUlps))
    {
      pass = false;
    }
  }
  return pass;
}

template <typename TVector>
bool
testVariableVector(const TVector & v1, const TVector & v2, int maxUlps = 4)
{
  bool               pass = true;
  const unsigned int D1 = v1.Size();
  const unsigned int D2 = v2.Size();

  if (D1 != D2)
  {
    return false;
  }
  for (unsigned int i = 0; i < D1; ++i)
  {
    if (!testValue(v1[i], v2[i], maxUlps))
    {
      pass = false;
    }
  }
  return pass;
}

} // namespace

int
itkAffineTransformTest(int, char *[])
{
  /* NOTE: The truth values for tests were taken from the output
      of the tests themselves. The assumption is that this code
      has been well-tested by other means already. */

  // Save the format stream variables for std::cout
  // They will be restored when coutState goes out of scope
  // scope.
  const itk::StdStreamStateSave coutState(std::cout);

  /* Set outstream precision */
  std::cout.precision(20);
  std::cerr.precision(20);

  int any = 0; // Any errors detected in testing?

  /* Create a 2D identity transformation and show its parameters */
  using Affine2DType = itk::AffineTransform<double, 2>;
  auto        id2 = Affine2DType::New();
  Matrix2Type matrix2 = id2->GetMatrix();
  Vector2Type vector2 = id2->GetOffset();
  std::cout << "Matrix from instantiating an identity transform:" << std::endl << matrix2;
  std::cout << "Vector from instantiating an identity transform:" << std::endl;
  PrintVector(vector2);

  Matrix2Type matrix2Truth;
  /* Test identity transform against truth */
  matrix2Truth.SetIdentity();
  Vector2Type vector2Truth({ 0, 0 });

  if (!testMatrix(matrix2, matrix2Truth) || !testVector(vector2, vector2Truth))
  {
    std::cout << "Default identity transformation test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Create and show a simple 2D transform from given parameters */
  matrix2[0][0] = 1;
  matrix2[0][1] = 2;
  matrix2[1][0] = 3;
  matrix2[1][1] = 4;
  vector2[0] = 5;
  vector2[1] = 6;

  auto aff2 = Affine2DType::New();
  aff2->SetMatrix(matrix2);
  aff2->SetOffset(vector2);
  for (unsigned int i = 0; i < 2; ++i)
  {
    for (unsigned int j = 0; j < 2; ++j)
    {
      matrix2[i][j] = 0.0;
    }
    vector2[i] = 0.0;
  }
  std::cout << "Instantiation of a given 2D transform:" << std::endl;
  aff2->Print(std::cout);

  /* Get and test inverse of whole transform */
  auto affInv2 = Affine2DType::New();
  if (!aff2->GetInverse(affInv2))
  {
    std::cout << "Test transform does not have an inverse when expected." << std::endl;
    return EXIT_FAILURE;
  }
  std::cout << "Inverse transform for the given transform:" << std::endl << affInv2;

  matrix2Truth[0][0] = -2;
  matrix2Truth[0][1] = 1;
  matrix2Truth[1][0] = 1.5;
  matrix2Truth[1][1] = -0.5;
  vector2Truth[0] = 4;
  vector2Truth[1] = -4.5;

  if (!testMatrix(affInv2->GetMatrix(), matrix2Truth, 7) || !testVector(affInv2->GetOffset(), vector2Truth, 6))
  {
    std::cout << "Inverse transform test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Set parameters of a 2D transform */
  matrix2[0][0] = 6;
  matrix2[0][1] = 5;
  matrix2[1][0] = 4;
  matrix2[1][1] = 3;
  vector2[0] = 2;
  vector2[1] = 1;
  aff2->SetMatrix(matrix2);
  aff2->SetOffset(vector2);
  for (unsigned int i = 0; i < 2; ++i)
  {
    for (unsigned int j = 0; j < 2; ++j)
    {
      matrix2[i][j] = 0.0;
    }
    vector2[i] = 0.0;
  }
  matrix2 = aff2->GetMatrix();
  vector2 = aff2->GetOffset();
  std::cout << "Setting the matrix in an existing transform:" << std::endl << matrix2;
  std::cout << "Setting the offset in an existing  transform:" << std::endl;
  PrintVector(vector2);

  /* Try composition of two transformations */
  aff2->Compose(aff2);
  std::cout << "Result of a composition:" << std::endl;
  aff2->Print(std::cout);

  matrix2Truth[0][0] = 56;
  matrix2Truth[0][1] = 45;
  matrix2Truth[1][0] = 36;
  matrix2Truth[1][1] = 29;
  vector2Truth[0] = 19;
  vector2Truth[1] = 12;

  if (!testMatrix(aff2->GetMatrix(), matrix2Truth) || !testVector(aff2->GetOffset(), vector2Truth))
  {
    std::cout << "Composition of two transformations test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Compose with a translation */
  Vector2Type trans;
  trans[0] = 1;
  trans[1] = 2;
  aff2->Translate(trans);
  std::cout << "Result of a translation:" << std::endl;
  aff2->Print(std::cout);

  matrix2Truth[0][0] = 56;
  matrix2Truth[0][1] = 45;
  matrix2Truth[1][0] = 36;
  matrix2Truth[1][1] = 29;
  vector2Truth[0] = 20;
  vector2Truth[1] = 14;

  if (!testMatrix(aff2->GetMatrix(), matrix2Truth) || !testVector(aff2->GetOffset(), vector2Truth))
  {
    std::cout << "Composition with a translation test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Compose with an isotropic scaling */
  aff2->Scale(.3, true);
  std::cout << "Result of isotropic scaling:" << std::endl;
  aff2->Print(std::cout);

  matrix2Truth[0][0] = 16.8;
  matrix2Truth[0][1] = 13.5;
  matrix2Truth[1][0] = 10.8;
  matrix2Truth[1][1] = 8.7;
  vector2Truth[0] = 20;
  vector2Truth[1] = 14;

  if (!testMatrix(aff2->GetMatrix(), matrix2Truth) || !testVector(aff2->GetOffset(), vector2Truth))
  {
    std::cout << "Composition with isotropic scaling test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Compose with an anisotropic scaling */
  Vector2Type scale;
  scale[0] = .3;
  scale[1] = .2;
  aff2->Scale(scale);
  std::cout << "Result of anisotropic scaling:" << std::endl;
  aff2->Print(std::cout);

  matrix2Truth[0][0] = 5.04;
  matrix2Truth[0][1] = 4.05;
  matrix2Truth[1][0] = 2.16;
  matrix2Truth[1][1] = 1.74;
  vector2Truth[0] = 6;
  vector2Truth[1] = 2.8;

  if (!testMatrix(aff2->GetMatrix(), matrix2Truth) || !testVector(aff2->GetOffset(), vector2Truth))
  {
    std::cout << "Composition with anisotropic scaling test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Compose with a general N-D rotation */
  aff2->Rotate(0, 1, 0.57, true);
  std::cout << "Result of general rotation:" << std::endl;
  aff2->Print(std::cout);

  matrix2Truth[0][0] = 2.0576711174453;
  matrix2Truth[0][1] = 6.1294444750264;
  matrix2Truth[1][0] = 0.87954634155339;
  matrix2Truth[1][1] = 2.6305129220477;
  vector2Truth[0] = 6;
  vector2Truth[1] = 2.8;

  if (!testMatrix(aff2->GetMatrix(), matrix2Truth, 100) || !testVector(aff2->GetOffset(), vector2Truth))
  {
    std::cout << "Composition with a general N-D rotation test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Compose with a 2-D rotation */
  aff2->Rotate(0, 1, -0.57, true);
  std::cout << "Result of 2-D rotation:" << std::endl;
  aff2->Print(std::cout);

  matrix2Truth[0][0] = 5.04;
  matrix2Truth[0][1] = 4.05;
  matrix2Truth[1][0] = 2.16;
  matrix2Truth[1][1] = 1.74;
  vector2Truth[0] = 6;
  vector2Truth[1] = 2.8;

  if (!testMatrix(aff2->GetMatrix(), matrix2Truth) || !testVector(aff2->GetOffset(), vector2Truth))
  {
    std::cout << "Composition with a 2-D rotation test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Compose with a shear */
  aff2->Shear(1, 0, .2);
  std::cout << "Result of shear:" << std::endl;
  aff2->Print(std::cout);

  matrix2Truth[0][0] = 5.04;
  matrix2Truth[0][1] = 4.05;
  matrix2Truth[1][0] = 3.168;
  matrix2Truth[1][1] = 2.55;
  vector2Truth[0] = 6;
  vector2Truth[1] = 4;

  if (!testMatrix(aff2->GetMatrix(), matrix2Truth) || !testVector(aff2->GetOffset(), vector2Truth))
  {
    std::cout << "Composition with a shear test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Transform a point */
  const itk::Point<double, 2> u2({ 3, 5 });
  itk::Point<double, 2>       v2 = aff2->TransformPoint(u2);
  std::cout << "Transform a point:" << std::endl << v2[0] << " , " << v2[1] << std::endl;

  itk::Point<double, 2> v2T({ 41.37, 26.254 });
  if (!testValue(v2[0], v2T[0]) || !testValue(v2[1], v2T[1]))
  {
    std::cout << "Transform a point test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Back transform a point */
  // v2 = aff2->BackTransform(u2);
  // std::cout << "Back transform a point:" << std::endl
  // << v2[0] << " , " << v2[1] << std::endl;

  /* Transform a vnl_vector */
  const vnl_vector_fixed<double, 2> x2{ 1, 2 };
  vnl_vector_fixed<double, 2>       y2 = aff2->TransformVector(x2);


  std::cout << "Transform a vnl_vector:" << std::endl << y2[0] << " , " << y2[1] << std::endl;
  vnl_vector_fixed<double, 2> y2T{ 13.14, 8.268 };
  if (!testValue(y2[0], y2T[0]) || !testValue(y2[1], y2T[1]))
  {
    std::cout << "Transform a vnl_vector test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Back transform a vector */
  // y2 = aff2->BackTransform(x2);
  // std::cout << "Back transform a vnl_vector:" << std::endl
  // << y2[0] << " , " << y2[1] << std::endl;

  /* Transform a vector */
  const itk::Vector<double, 2> u3({ 3, 5 });
  itk::Vector<double, 2>       v3 = aff2->TransformVector(u3);
  std::cout << "Transform a vector:" << std::endl << v3[0] << " , " << v3[1] << std::endl;

  itk::Vector<double, 2> v3T({ 35.37, 22.254 });
  if (!testVector(v3, v3T))
  {
    std::cout << "Transform a vector test failed." << std::endl;
    return EXIT_FAILURE;
  }

  v3 = aff2->TransformVector(u3, u2);
  std::cout << "Transform a vector with a point:" << std::endl << v3[0] << " , " << v3[1] << std::endl;

  v3T[0] = 35.37;
  v3T[1] = 22.254;
  if (!testVector(v3, v3T))
  {
    std::cout << "Transform a vector with a point test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Transform a variable length vector */
  itk::VariableLengthVector<double> l3;
  l3.SetSize(2);
  itk::VariableLengthVector<double> m3T;
  m3T.SetSize(2);
  itk::VariableLengthVector<double> m3;
  l3[0] = 3;
  l3[1] = 5;
  m3 = aff2->TransformVector(l3);
  std::cout << "Transform a variable length vector:" << std::endl << m3[0] << " , " << m3[1] << std::endl;

  m3T[0] = 35.37;
  m3T[1] = 22.254;
  if (!testVariableVector(m3, m3T))
  {
    std::cout << "Transform a variable length vector test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Back transform a vector */
  // v3 = aff2->BackTransform(u3);
  // std::cout << "Back transform a vector :" << std::endl
  // << v3[0] << " , " << v3[1] << std::endl;

  /* Transform a Covariant vector */
  itk::CovariantVector<double, 2> u4;
  u4[0] = 3;
  u4[1] = 5;
  itk::CovariantVector<double, 2> v4 = aff2->TransformCovariantVector(u4);
  std::cout << "Transform a Covariant vector:" << std::endl << v4[0] << " , " << v4[1] << std::endl;


  itk::CovariantVector<double, 2> v4T;
  v4T[0] = -379.16666666679;
  v4T[1] = 604.16666666687;
  if (!testVector(v4, v4T, 4000))
  {
    std::cout << "Transform a covariant vector test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Transform a variable length vector as covariant vector */
  itk::VariableLengthVector<double> l4;
  l4.SetSize(2);
  itk::VariableLengthVector<double> m4T;
  m4T.SetSize(2);
  l4[0] = 3;
  l4[1] = 5;
  itk::VariableLengthVector<double> m4;
  m4 = aff2->TransformCovariantVector(l4);
  std::cout << "Transform a variable length covariant vector:" << std::endl << m4[0] << " , " << m4[1] << std::endl;

  m4T[0] = -379.16666666679;
  m4T[1] = 604.16666666687;
  if (!testVariableVector(m4, m4T, 4000))
  {
    std::cout << "Transform a variable length covariant vector test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Back transform a vector */
  // v4 = aff2->BackTransform(u4);
  // std::cout << "Back transform a vector :" << std::endl
  // << v4[0] << " , " << v4[1] << std::endl;

  using Affine3DType = itk::AffineTransform<double, 3>;
  Affine3DType::MatrixType matrix3Truth;

  /* Create a 3D transform and rotate in 3D */
  const itk::Vector<double, 3> axis({ .707, .707, .707 });
  auto                         aff3 = Affine3DType::New();
  aff3->Rotate3D(axis, 1.0, true);
  std::cout << "Create and rotate a 3D transform:" << std::endl;
  aff3->Print(std::cout);

  matrix3Truth[0][0] = 0.69353487057876;
  matrix3Truth[0][1] = -0.33259093488348;
  matrix3Truth[0][2] = 0.63905606430472;
  matrix3Truth[1][0] = 0.63905606430472;
  matrix3Truth[1][1] = 0.69353487057876;
  matrix3Truth[1][2] = -0.33259093488348;
  matrix3Truth[2][0] = -0.33259093488348;
  matrix3Truth[2][1] = 0.63905606430472;
  matrix3Truth[2][2] = 0.69353487057876;

  if (!testMatrix(aff3->GetMatrix(), matrix3Truth, 30))
  {
    std::cout << "3D transform rotation test failed." << std::endl;
    return EXIT_FAILURE;
  }

  /* Generate inverse transform */
  auto inv3 = Affine3DType::New();
  if (!aff3->GetInverse(inv3))
  {
    std::cout << "Cannot compute inverse transformation" << std::endl;
    return EXIT_FAILURE;
  }
  std::cout << "Create an inverse transformation:" << std::endl;
  inv3->Print(std::cout);

  matrix3Truth[0][0] = 0.69353487057876;
  matrix3Truth[0][1] = 0.63905606430472;
  matrix3Truth[0][2] = -0.33259093488348;
  matrix3Truth[1][0] = -0.33259093488348;
  matrix3Truth[1][1] = 0.69353487057876;
  matrix3Truth[1][2] = 0.63905606430472;
  matrix3Truth[2][0] = 0.63905606430472;
  matrix3Truth[2][1] = -0.33259093488348;
  matrix3Truth[2][2] = 0.69353487057876;

  if (!testMatrix(inv3->GetMatrix(), matrix3Truth, 40))
  {
    std::cout << "Compute inverse test failed." << std::endl;
    return EXIT_FAILURE;
  }

  const Affine3DType::Pointer inv4 = dynamic_cast<Affine3DType *>(aff3->GetInverseTransform().GetPointer());
  if (!inv4)
  {
    std::cout << "Cannot compute inverse transformation inv4" << std::endl;
    return EXIT_FAILURE;
  }
  std::cout << "Create an inverse transformation:" << std::endl;
  inv4->Print(std::cout);

  if (!testMatrix(inv4->GetMatrix(), matrix3Truth, 35))
  {
    std::cout << "Compute inverse test failed - inv4." << std::endl;
    return EXIT_FAILURE;
  }

  /* Test output of ComputeJacobianWithRespectToParameters */
  auto jaff = Affine3DType::New();

  Affine3DType::InputPointType jpoint;
  jpoint[0] = 5.0;
  jpoint[1] = 10.0;
  jpoint[2] = 15.0;
  Affine3DType::JacobianType jaffJacobian;
  jaff->ComputeJacobianWithRespectToParameters(jpoint, jaffJacobian);

  std::cout << "ComputeJacobianWithRespectToParameters: " << std::endl;
  std::cout << jaffJacobian << std::endl;

  constexpr double data[]{ 5, 10, 15, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,  0,  5, 10, 15,
                           0, 0,  0,  0, 1, 0, 0, 0, 0, 0, 0, 0, 5, 10, 15, 0, 0,  1 };

  const vnl_matrix<double>   vnlData(data, 3, 12);
  Affine3DType::JacobianType expectedJacobian(vnlData);
  for (unsigned int i = 0; i < 3; ++i)
  {
    for (unsigned int j = 0; j < 12; ++j)
    {
      if (!testValue(expectedJacobian[i][j], jaffJacobian[i][j]))
      {
        std::cout << "ComputeJacobianWithRespectToParameters test failed." << std::endl;
        return EXIT_FAILURE;
      }
    }
  }

  /* Test ComputeJacobianWithRespectToPosition. Should return Matrix. */
  matrix3Truth[0][0] = 1;
  matrix3Truth[0][1] = 2;
  matrix3Truth[0][2] = 3;
  matrix3Truth[1][0] = 4;
  matrix3Truth[1][1] = 5;
  matrix3Truth[1][2] = 6;
  matrix3Truth[2][0] = 7;
  matrix3Truth[2][1] = 8;
  matrix3Truth[2][2] = 8;

  jaff->SetMatrix(matrix3Truth);
  Affine3DType::JacobianPositionType jaffJacobianPosition;
  jaff->ComputeJacobianWithRespectToPosition(jpoint, jaffJacobianPosition);
  for (unsigned int i = 0; i < Affine3DType::MatrixType::RowDimensions; ++i)
  {
    for (unsigned int j = 0; j < Affine3DType::MatrixType::ColumnDimensions; ++j)
    {
      if (!testValue(jaffJacobianPosition[i][j], matrix3Truth[i][j]))
      {
        std::cout << "Failed ComputeJacobianWithRespectToPosition." << std::endl
                  << "jaffJacobianPosition: " << jaffJacobianPosition << std::endl
                  << "matrix3Truth: " << matrix3Truth << std::endl;
        return EXIT_FAILURE;
      }
    }
  }

  /* Test ComputeInverseJacobianWithRespectToPosition. Should return the inverse Matrix. */
  Matrix3Type matrix3invTruth;
  matrix3invTruth[0][0] = -8. / 3.;
  matrix3invTruth[0][1] = 8. / 3.;
  matrix3invTruth[0][2] = -1;
  matrix3invTruth[1][0] = 10. / 3.;
  matrix3invTruth[1][1] = -13. / 3.;
  matrix3invTruth[1][2] = 2;
  matrix3invTruth[2][0] = -1;
  matrix3invTruth[2][1] = 2;
  matrix3invTruth[2][2] = -1;
  Affine3DType::JacobianPositionType jaffInverseJacobianPosition;
  jaff->ComputeInverseJacobianWithRespectToPosition(jpoint, jaffInverseJacobianPosition);
  for (unsigned int i = 0; i < Affine3DType::MatrixType::RowDimensions; ++i)
  {
    for (unsigned int j = 0; j < Affine3DType::MatrixType::ColumnDimensions; ++j)
    {
      if (itk::Math::abs(jaffInverseJacobianPosition[i][j] - matrix3invTruth[i][j]) > 1e-13)
      {
        std::cout << "Failed ComputeInverseJacobianWithRespectToPosition." << std::endl
                  << "jaffInverseJacobianPosition: " << jaffInverseJacobianPosition << std::endl
                  << "matrix3invTruth: " << matrix3invTruth << std::endl;
        return EXIT_FAILURE;
      }
    }
  }

  /* Test SetParameters */
  auto paff = Affine3DType::New();
  paff->Print(std::cout);
  Affine3DType::ParametersType parameters1(paff->GetNumberOfParameters());
  Affine3DType::ParametersType fixed_parameters = paff->GetFixedParameters();
  const size_t                 fixed_params_size = fixed_parameters.Size();
  for (unsigned int q = 0; q < fixed_params_size; ++q)
  {
    fixed_parameters[q] = 100.0 + q;
  }
  paff->SetFixedParameters(fixed_parameters);

  /* set up a 3x3 magic square matrix */
  parameters1[0] = 8;
  parameters1[1] = 1;
  parameters1[2] = 6;
  parameters1[3] = 3;
  parameters1[4] = 5;
  parameters1[5] = 7;
  parameters1[6] = 4;
  parameters1[7] = 9;
  parameters1[8] = 2;

  parameters1[9] = 5;
  parameters1[10] = 5;
  parameters1[11] = 5;

  paff->SetParameters(parameters1);
  paff->Print(std::cout);

  // TEST INVERSE OF INVERSE
  auto paff_inv = Affine3DType::New();
  paff->GetInverse(paff_inv);
  auto paff_inv_inv = Affine3DType::New();
  paff_inv->GetInverse(paff_inv_inv);

  std::cout << "TEST INVERSE" << std::endl;
  paff_inv->Print(std::cout);
  std::cout << "TEST INVERSE OF INVERSE" << std::endl;
  paff_inv_inv->Print(std::cout);

  bool found_inv_inv_descrepancies = false;
  {
    Affine3DType::ParametersType parameters1_inv_inv = paff_inv_inv->GetParameters();
    // Check that Inv(Inv(T)) ~= T
    double mag_error = 0;
    for (unsigned int q = 0; q < parameters1_inv_inv.size(); ++q)
    {
      const double v = (parameters1[q] - parameters1_inv_inv[q]);
      // Protect against numerical errors when v~=-0 with abs.
      mag_error += sqrt(itk::Math::abs(v));
    }
    if (mag_error > 1e-4)
    {
      std::cout << "ERROR: Moving Parameters do not match!" << std::endl;
      std::cout << parameters1 << std::endl;
      std::cout << parameters1_inv_inv << std::endl;
      found_inv_inv_descrepancies = true;
    }
  }
  {
    Affine3DType::ParametersType fixed_parameters_inv_inv = paff_inv_inv->GetFixedParameters();
    double                       mag_error = 0;
    for (unsigned int q = 0; q < fixed_parameters_inv_inv.size(); ++q)
    {
      const double v = (fixed_parameters[q] - fixed_parameters_inv_inv[q]);
      mag_error += sqrt(v);
    }
    if (mag_error > 1e-4)
    {
      std::cout << "ERROR: Fixed Parameters do not match!" << std::endl;
      std::cout << fixed_parameters << std::endl;
      std::cout << fixed_parameters_inv_inv << std::endl;
      found_inv_inv_descrepancies = true;
    }
  }
  if (found_inv_inv_descrepancies)
  {
    std::cout << "ERROR: Inverse of Inverse does not match original!" << std::endl;
    return EXIT_FAILURE;
  }

  Affine3DType::ParametersType parametersRead(paff->GetNumberOfParameters());
  parametersRead = paff->GetParameters();
  for (unsigned int k = 0; k < paff->GetNumberOfParameters(); ++k)
  {
    if (!testValue(parameters1[k], parametersRead[k]))
    {
      std::cout << "SetParameters test failed." << std::endl;
      return EXIT_FAILURE;
    }
  }

  /* Test UpdateTransformParameters */
  Affine3DType::DerivativeType update(paff->GetNumberOfParameters());
  Affine3DType::ParametersType updateTruth;
  updateTruth = parameters1;
  for (unsigned int i = 0; i < paff->GetNumberOfParameters(); ++i)
  {
    update[i] = i / 10.0;
    updateTruth[i] += update[i];
  }
  /* Update all the parameters, with default scaling factor of 1 */
  paff->UpdateTransformParameters(update);
  parametersRead = paff->GetParameters();
  for (unsigned int k = 0; k < paff->GetNumberOfParameters(); ++k)
  {
    if (itk::Math::NotAlmostEquals(updateTruth[k], parametersRead[k]))
    {
      std::cout << "UpdateTransformParameters 1 failed." << std::endl;
      std::cout << "updateTruth: " << std::endl
                << updateTruth << std::endl
                << "parametersRead: " << std::endl
                << parametersRead << std::endl;
      return EXIT_FAILURE;
    }
  }
  /* Update with a non-unit scaling factor */
  constexpr double factor{ 0.5 };
  for (unsigned int i = 0; i < paff->GetNumberOfParameters(); ++i)
  {
    update[i] = i;
    updateTruth[i] += update[i] * factor;
  }
  paff->UpdateTransformParameters(update, factor);
  parametersRead = paff->GetParameters();
  for (unsigned int k = 0; k < paff->GetNumberOfParameters(); ++k)
  {
    if (itk::Math::NotAlmostEquals(updateTruth[k], parametersRead[k]))
    {
      std::cout << "UpdateTransformParameters 2 failed." << std::endl;
      return EXIT_FAILURE;
    }
  }

  paff->SetIdentity();
  paff->Print(std::cout);

  {
    // Test SetParameters and GetInverse
    using TransformType = itk::AffineTransform<double, 2>;
    auto transform = TransformType::New();

    TransformType::ParametersType parameters2;
    TransformType::ParametersType expectedParameters;
    expectedParameters.SetSize(transform->GetNumberOfParameters());

    // check the returned parameters

    // Test 1: SetIdentity
    transform->SetIdentity();
    parameters2 = transform->GetParameters();

    expectedParameters.Fill(0.0);
    expectedParameters[0] = 1.0;
    expectedParameters[3] = 1.0;
    for (unsigned int k = 0; k < transform->GetNumberOfParameters(); ++k)
    {
      if (!testValue(parameters2[k], expectedParameters[k]))
      {
        std::cout << "Test failed:" << std::endl;
        std::cout << "Results=" << parameters2 << std::endl;
        std::cout << "Expected=" << expectedParameters << std::endl;
        any = true;
        break;
      }
    }

    // Test 2: SetParameters
    expectedParameters.Fill(0.0);
    expectedParameters[0] = 2.0;
    expectedParameters[3] = 2.0;

    transform->SetParameters(expectedParameters);
    parameters2 = transform->GetParameters();
    for (unsigned int k = 0; k < transform->GetNumberOfParameters(); ++k)
    {
      if (!testValue(parameters2[k], expectedParameters[k]))
      {
        std::cout << "Test failed:" << std::endl;
        std::cout << "Results=" << parameters2 << std::endl;
        std::cout << "Expected=" << expectedParameters << std::endl;
        any = true;
        break;
      }
    }

    // Test 3: GetInverse
    expectedParameters.Fill(0.0);
    expectedParameters[0] = 2.0;
    expectedParameters[3] = 2.0;

    transform->SetParameters(expectedParameters);

    auto other = TransformType::New();
    transform->GetInverse(other);

    const TransformType::Pointer otherbis =
      dynamic_cast<TransformType *>(transform->GetInverseTransform().GetPointer());

    parameters2 = other->GetParameters();
    TransformType::ParametersType parameters2bis = otherbis->GetParameters();

    expectedParameters.Fill(0.0);
    expectedParameters[0] = 0.5;
    expectedParameters[3] = 0.5;

    other->Print(std::cout);
    otherbis->Print(std::cout);
    for (unsigned int k = 0; k < transform->GetNumberOfParameters(); ++k)
    {
      if (!testValue(parameters2[k], expectedParameters[k]))
      {
        std::cout << "Test failed:" << std::endl;
        std::cout << "Results=" << parameters2 << std::endl;
        std::cout << "Expected=" << expectedParameters << std::endl;
        any = true;
        break;
      }
    }
    for (unsigned int k = 0; k < transform->GetNumberOfParameters(); ++k)
    {
      if (!testValue(parameters2bis[k], expectedParameters[k]))
      {
        std::cout << "Test failed:" << std::endl;
        std::cout << "Results=" << parameters2bis << std::endl;
        std::cout << "Expected=" << expectedParameters << std::endl;
        any = true;
        break;
      }
    }

    // Try to invert a singular transform
    auto singularTransform = TransformType::New();
    auto singularTransformInverse = TransformType::New();
    singularTransform->Scale(0.0);
    if (!singularTransform->GetInverse(singularTransformInverse))
    {
      std::cout << "Detected an attempt to invert a singular transform as expected" << std::endl;
    }
    else
    {
      std::cout << "Failed to detect an attempt to invert a singular transform!" << std::endl;
      return EXIT_FAILURE;
    }

    const TransformType::Pointer singularTransformInverse2 =
      dynamic_cast<TransformType *>(singularTransform->GetInverseTransform().GetPointer());
    if (!singularTransformInverse2)
    {
      std::cout << "Detected an attempt to invert a singular transform as expected" << std::endl;
    }
    else
    {
      std::cout << "Failed to detect an attempt to invert a singular transform!" << std::endl;
      return EXIT_FAILURE;
    }
  }

  return any;
}
